function [out_sim] = sim_icastep(in_sim, vehicleIsStatic)
%SIM_ICASTEP icastep仿真函数

if (nargin<1) || isempty(in_sim)% 不处于蒙特卡罗仿真循环内
    cfg_sim.inMCLoop = false;
    in_sim.curGrpIdx = 1;
    in_sim.curSimIdx = 1;
    tmp_sim = load('mcsim_ica', 'in');
    in_sim.parRefVal = tmp_sim.in.par.refVal;
    in_sim.curSimParVal = [in_sim.parRefVal(1:28, 2); in_sim.parRefVal(29:31, 1); in_sim.parRefVal(32, 2); in_sim.parRefVal(33:end, 1)];
    in_sim.CWD = pwd;
else
    cfg_sim.inMCLoop = true;
end

if nargin < 2
    vehicleIsStatic = false;
end

%% 仿真设置
% 常量
cst_sim.sec2Rad = pi/3600/180;
cst_sim.deg2Rad = pi/180;

% 参量
par_sim.samplePeriod = 0.005;
par_sim.g = 9.793247269375176;

%% 生成轨迹
cst_trajGen = load('earthconstants_WGS84.mat', 'omegaIE', 'RE', 'eSq', 'f');

cfg_trajGen.enableTrajGen = true;
cfg_trajGen.IMU.enableErr = true;
cfg_trajGen.IMU.enableNoise = true;
cfg_trajGen.IMU.quantizeOut = false;
cfg_trajGen.enableNavigation = true;

par_trajGen.samplePeriod = par_sim.samplePeriod;
par_trajGen.g = par_sim.g; % 重力加速度，m/s^2
% 加速度计常值误差
par_trajGen.IMU.acc.KScal0 = ones(3, 1); % 转换前单位(^/s)/(g)，转换后单位(^/s)/(m/s^2)
par_trajGen.IMU.acc.dfBiasS = 1*in_sim.curSimParVal(1:3) * 1e-6 * par_trajGen.g;
par_trajGen.IMU.acc.dKScalP = [1*in_sim.curSimParVal(4:6) * 1e-6 1*in_sim.curSimParVal(13:15) * 1e-6 / par_trajGen.g];
par_trajGen.IMU.acc.dKScalN = par_trajGen.IMU.acc.dKScalP;
par_trajGen.IMU.acc.dPSS0 = 1*vec2offdiag(in_sim.curSimParVal(7:12))' * 1e-6;
% 加速度计随机误差
par_trajGen.IMU.acc.noise.R = in_sim.curSimParVal(28) * ones(3, 1) * (par_trajGen.g/1e6) / 3600; % 转换前单位μg/h，转换后单位m/s^3
par_trajGen.IMU.acc.noise.K = in_sim.curSimParVal(29) * ones(3, 1) * (par_trajGen.g/1e6) / 60; % 转换前单位μg/sqrt(h)，转换后单位m/s^(5/2)
par_trajGen.IMU.acc.noise.N = in_sim.curSimParVal(30) * ones(3, 1) * (par_trajGen.g/1e6) * 60; % 转换前单位μg*sqrt(h)，转换后单位m/s^(3/2)
par_trajGen.IMU.acc.noise.Q = in_sim.curSimParVal(31) * ones(3, 1) / 3600; % 转换前单位m/h，转换后单位m/s
% 陀螺常值误差
par_trajGen.IMU.gyro.KScal0 = ones(3, 1); % 转换前单位(^/s)/(°/h)，转换后单位(^/s)/(rad/s)
par_trajGen.IMU.gyro.domegaIBBiasS = 1*in_sim.curSimParVal(16:18) * cst_sim.sec2Rad ; % 转换前单位°/h
par_trajGen.IMU.gyro.dKScalP = 1*in_sim.curSimParVal(19:21) * 1e-6;
par_trajGen.IMU.gyro.dKScalN = par_trajGen.IMU.gyro.dKScalP;
par_trajGen.IMU.gyro.dPSS0 =  1*vec2offdiag(in_sim.curSimParVal(22:27))' * 1e-6;
% 陀螺随机误差
par_trajGen.IMU.gyro.noise.R = in_sim.curSimParVal(32) * ones(3, 1) * cst_sim.deg2Rad / (3600^2); % 转换前单位°/h^2，转换后单位rad/s^2
par_trajGen.IMU.gyro.noise.K = in_sim.curSimParVal(33) * ones(3, 1) * cst_sim.deg2Rad / (3600^(3/2)); % 转换前单位°/h^(3/2)，转换后单位rad/s^(3/2)
par_trajGen.IMU.gyro.noise.N = in_sim.curSimParVal(34) * ones(3, 1) * cst_sim.deg2Rad / 60; % 转换前单位°/sqrt(h)，转换后单位rad/sqrt(s)
par_trajGen.IMU.gyro.noise.Q = in_sim.curSimParVal(35) * ones(3, 1) * cst_sim.sec2Rad; % 转换前单位″，转换后单位rad
% par_trajGen.IMU.lV = [1 1 10]';
% 输入
if vehicleIsStatic
    %     in_trajGen.genFuncName = 'gentgi_static';
    %     in_trajGen.genFuncInArg = {300};
    
    [ ~, in_trajGen.omegaVBB, ~, in_trajGen.tSects ] = gentgi_static(300);
    rotMagnitude = [3 3 100]*pi/180; % 摇摆轨迹 纵摇、横摇、艏摇
    rotPeriod = [56 56 250];
    iniPhase = [0 0 0];
    slopeCoff = [0 0 0.0026/180*pi];
    [in_trajGen.vVRate, in_trajGen.omegaNVV, ~] = gentgi_rocking(rotMagnitude, rotPeriod, iniPhase, in_trajGen.omegaVBB.time(end), slopeCoff);
    
    %     % 方位角转动
    %     in_trajGen.aligntimeSec = [ 1 1 ]; %
    %     in_trajGen.rotVec = [ 0 0 pi*100/180 ]; % 双位置转动矢量
    %     in_trajGen.genFuncName = 'gentgi_statmultipos';
    %     in_trajGen.genFuncInArg = {in_trajGen.rotVec, pi/300, in_trajGen.aligntimeSec,0/3600};
else
%     rotMagnitude = 0*[ 6 8 7 ]*pi/180; % 纵摇、横摇、艏摇
%     rotPeriod = [ 10 11 12 ];
%     velMagnitude = 0*[ 1.0 1.1 1.2 ]; % 横荡、纵荡、垂荡
%     velPeriod = [ 4 5 6 ];
%     span = 200;
%     in_trajGen.genFuncName = 'gentgi_voyage';
%     in_trajGen.genFuncInArg = { rotMagnitude, rotPeriod, velMagnitude, velPeriod, span };
    transUniVec = [0 1 0]';
    rotVec = zeros(3, 0);
    transTimeVec = [10 200 10];
    in_trajGen.genFuncName = 'gentgi_navtest_L_racetrack';
    in_trajGen.genFuncInArg = { transUniVec, rotVec, transTimeVec };
    
end
% 初始条件
iniCon_trajGen.CVG = angle2dcm_cg(-0*cst_sim.deg2Rad, -0*cst_sim.deg2Rad, -0*cst_sim.deg2Rad,'ZXY')'; % 初始姿态矩阵，向东为[0 1 0; -1 0 0 ; 0 0 1 ];
% iniCon_trajGen.CVG = angle2dcm(0*pi/2, 2/180*pi, -8/180*pi, 'ZYX');
iniCon_trajGen.CBV = eye(3);
iniCon_trajGen.vG = [0 0 0]'; % 初始速度
iniCon_trajGen.pos = [30*cst_sim.deg2Rad, 110*cst_sim.deg2Rad, 100]'; % 纬度、经度、高度

[out_trajGen, ~]= gentrajectory(in_trajGen, cfg_trajGen, par_trajGen, iniCon_trajGen, cst_trajGen); %#ok<*NCOMMA>
% 提取部分轨迹数据
bgnIdx = 20/par_sim.samplePeriod;
endIdx = bgnIdx + 180/par_sim.samplePeriod;
out_trajGen.t = out_trajGen.t(1:endIdx-bgnIdx+1, :);
out_trajGen.pos = out_trajGen.pos(bgnIdx:endIdx, :, :);
out_trajGen.vG = out_trajGen.vG(bgnIdx:endIdx, :, :);
out_trajGen.CBG = out_trajGen.CBG(:, :, bgnIdx:endIdx);
out_trajGen.CBL = out_trajGen.CBL(:, :, bgnIdx:endIdx);
out_trajGen.IMU.accOut = out_trajGen.IMU.accOut(bgnIdx:endIdx-1, :);
out_trajGen.IMU.gyroOut = out_trajGen.IMU.gyroOut(bgnIdx:endIdx-1, :);
% 提取参考值
refNav.t = out_trajGen.t;
refNav.pos = out_trajGen.pos(:, :, 3);
refNav.vG = out_trajGen.vG(:, :, 3);
refNav.CBG = out_trajGen.CBG;
refNav.CBL = out_trajGen.CBL;

if ~cfg_sim.inMCLoop && ispc && ~vehicleIsStatic
%     figure;plot(out_trajGen.t, out_trajGen.omegaNBB*180/pi);grid on;xlabel('t/(s)');ylabel('角速度/(°/s)');legend('X', 'Y', 'Z');
%     figure;plot(out_trajGen.t, out_trajGen.bodyAtt*180/pi);grid on;xlabel('t/(s)');ylabel('姿态角/(°)');legend('Z', 'Y', 'X');
%     figure;plot(out_trajGen.t, out_trajGen.pos(:,end));grid on;xlabel('t/(s)');ylabel('高度/(m)');
%     figure;plot(out_trajGen.t, out_trajGen.vG);grid on;xlabel('t/(s)');ylabel('速度/(m/s)');
    preparefig('运动轨迹'); plot(refNav.pos(:, 2)./pi*180, refNav.pos(:, 1)./pi*180); grid on; xlabel('纬度/度');ylabel('经度/度');
    preparefig('运动速度'); plot(refNav.t, refNav.vG);grid on; xlabel('t/(s)'); ylabel('速度/(m/s)'); legend('E', 'N', 'U');
end

%% 初始化对准
sampleNum = size(out_trajGen.IMU.accOut, 1);
clear('icastep');
in_ICAS.specVelInc = NaN(3, 1);
in_ICAS.angleInc = NaN(3, 1);
in_ICAS.vN = refNav.vG(1, :)';
in_ICAS.L = refNav.pos(1, 1);
in_ICAS.lambda = refNav.pos(1, 2);
in_ICAS.h = refNav.pos(1, 3);
in_ICAS.g = par_sim.g;
in_ICAS.doPolyFit = false;
cfg_ICAS.vehicleIsStatic = vehicleIsStatic;
cfg_ICAS.bufLen = int32(ceil(length(refNav.t)/60));
cfg_ICAS.bufInterval = int32(60);
cfg_ICAS.calcMethod = uint16(1);
cfg_ICAS.useSimpDlbVecOrient = false;
cfg_ICAS.latEstIntLen = int32(refNav.t(end)/2/par_sim.samplePeriod);
par_ICAS.samplePeriod = par_sim.samplePeriod;
par_ICAS.t1 = round(refNav.t(end)/2);
par_ICAS.vecInterval = int32(1/par_ICAS.samplePeriod);
cst_ICAS.omegaIE = cst_trajGen.omegaIE;
cst_ICAS.RE = cst_trajGen.RE;
cst_ICAS.epsilonSq = cst_trajGen.eSq;
[out_ICAS, auxOut_ICAS] = icastep(int8(0), in_ICAS, cfg_ICAS, par_ICAS, cst_ICAS);
out_ICAS = repmat(out_ICAS, sampleNum, 1);
auxOut_ICAS = repmat(auxOut_ICAS, sampleNum, 1);
% lV = [0.8 0.8 9.5; 0.8 0.8 9.5; 0.8 0.8 9.5];

%% 对准解算周期
out_CARV2.CBL = NaN(3, 3, sampleNum);
out_CARV2.azm = NaN(sampleNum, 3);
accOutSum = zeros(3, 1);
gyroOutSum = zeros(3, 1);

if ~cfg_sim.inMCLoop && ispc
    hwb = waitbar(0, '惯性系对准 Computation...');
end
for i = 1:sampleNum
    %
    if (mod(i, floor(sampleNum/100))==0) && ~cfg_sim.inMCLoop
        if ispc
            waitbar(double(i)/double(sampleNum), hwb);
        elseif isunix
            disp(['惯性系对准 Computation...Completed ', int2str(100*double(i)/double(sampleNum)), ' %']);
        end
    end
    in_ICAS.specVelInc = out_trajGen.IMU.accOut(i, 1:3)';    
    in_ICAS.angleInc = out_trajGen.IMU.gyroOut(i, 1:3)';
%     if i >= 2
%         in_ICAS.specVelInc = in_ICAS.specVelInc - acclevarmerr_inc_simp(lV', out_trajGen.IMU.gyroOut(i, 1:3)', out_trajGen.IMU.gyroOut(i-1, 1:3)', par_ICAS.samplePeriod);
%     end
    in_ICAS.vN = refNav.vG(i+1, :)' + 0.1/3*randn(3,1);
    in_ICAS.L = refNav.pos(i+1, 1) + 20/3/6378137*randn(1,1);
    in_ICAS.lambda = refNav.pos(i+1, 2) + 20/3/6378137*randn(1,1);
    in_ICAS.h = refNav.pos(i+1, 3) + 20/3*randn(1,1);
    in_ICAS.g = par_sim.g;
    
    if vehicleIsStatic
        % 解析粗对准
        accOutSum = accOutSum + in_ICAS.specVelInc;
        gyroOutSum = gyroOutSum + in_ICAS.angleInc;
        accOutMean = accOutSum / i;
        gyroOutMean = gyroOutSum / i;
        tmp_CBL = coarsealign_stat_rv2(accOutMean / par_ICAS.samplePeriod, gyroOutMean / par_ICAS.samplePeriod);
        out_CARV2.CBL(:, :, i) = tmp_CBL;
        out_CARV2.azm(i, :) = bodyaxisazimuth(tmp_CBL);
        % 惯性系对准设置
        if (i*par_ICAS.samplePeriod > 1)
            par_ICAS.t1 = i*par_ICAS.samplePeriod/2;
        end
        if mod(i, round(1/par_ICAS.samplePeriod)) == 0
            in_ICAS.doPolyFit = true;
        else
            in_ICAS.doPolyFit = false;
        end
    end
    % 惯性系对准
    [out_ICAS(i), auxOut_ICAS(i)] = icastep(int8(1), in_ICAS, cfg_ICAS, par_ICAS, cst_ICAS);
end
out_ICAS = structarray2structofarray(out_ICAS);
out_ICAS.azm = bodyaxisazimuth(out_ICAS.CBL);
auxOut_ICAS = structarray2structofarray(auxOut_ICAS);
%
if ~cfg_sim.inMCLoop && ispc
    close(hwb);
end

%% 绘图显示
close('all');
% 原始脉冲数
preparefig('原始脉冲数');
subplot(2, 1, 1);
plot(refNav.t(2:end, :), out_trajGen.IMU.accOut);
xlabel('t(s)'); ylabel('加速度计原始脉冲数(\^)');
legend('X加表', 'Y加表', 'Z加表');
subplot(2, 1, 2);
plot(refNav.t(2:end, :), out_trajGen.IMU.gyroOut);
xlabel('t(s)');
ylabel('陀螺原始脉冲数(\^)');
legend('X陀螺', 'Y陀螺', 'Z陀螺');
% % 零偏误差
% omegaIBB_err = out_trajGen.IMU.omegaIBB - out_trajGen.IMU.omegaIBBErrFree;
% omegaIBB_err_N = NaN(size(omegaIBB_err));
% for i = 1:length(omegaIBB_err)
%     omegaIBB_err_N(i, :) = (out_trajGen.CBG(:, :, i)*omegaIBB_err(i, :)')';
% end
% preparefig('陀螺零偏误差');
% subplot(2, 1, 1);
% plot(out_trajGen.t, omegaIBB_err/(pi/3600/180));
% xlabel('t(s)');
% ylabel('体系陀螺零偏误差(°/h)');
% legend('X陀螺', 'Y陀螺', 'Z陀螺');
% subplot(2, 1, 2);
% plot(out_trajGen.t, omegaIBB_err_N/(pi/3600/180));
% xlabel('t(s)');
% ylabel('导航系陀螺零偏误差(°/h)');
% legend('东向', '北向', '天向');
% 惯组X、Y轴水平投影北向方位角
preparefig('北向方位角');
subplot(2, 1, 1);
plot(refNav.t(2:end, :), [out_CARV2.azm(:, 1) out_ICAS.azm(:, 1)]./pi*180,'LineWidth',3);grid on
xlabel('t(s)');
ylabel('X轴北向方位角(°)');
legend('解析对准', '惯性系对准');
subplot(2, 1, 2);
plot(refNav.t(2:end, :), [out_CARV2.azm(:, 2) out_ICAS.azm(:, 2)]./pi*180,'LineWidth',3);grid on
xlabel('t(s)');
ylabel('Y轴北向方位角(°)');
legend('解析对准', '惯性系对准');
% L系下姿态误差
phiL_CARV2 = dcmdiff(out_CARV2.CBL, refNav.CBL(:, :, 2:end));
phiL_ICAS = dcmdiff(out_ICAS.CBL, refNav.CBL(:, :, 2:end));
preparefig('L系下姿态误差');
subplot(3, 1, 1);
plot(refNav.t(2:end, :), [phiL_CARV2(:, 1) phiL_ICAS(:, 1)]./pi*180*3600,'LineWidth',3);grid on
xlabel('t(s)');
ylabel('姿态误差\phi_x^L(″)');
legend('解析对准', '惯性系对准');
subplot(3, 1, 2);
plot(refNav.t(2:end, :), [phiL_CARV2(:, 2) phiL_ICAS(:, 2)]./pi*180*3600,'LineWidth',3);grid on
xlabel('t(s)');
ylabel('姿态误差\phi_y^L(″)');
legend('解析对准', '惯性系对准');
subplot(3, 1, 3);
plot(refNav.t(2:end, :), [phiL_CARV2(:, 3) phiL_ICAS(:, 3)]./pi*180*3600,'LineWidth',3);grid on
xlabel('t(s)');
ylabel('姿态误差\phi_z^L(″)');
legend('解析对准', '惯性系对准');
% 纬度估计值
preparefig('纬度估计值');
subplot(1, 2, 1);
plot(refNav.t(2:end, :), auxOut_ICAS.L/pi*180,'LineWidth',3);grid on
xlabel('t/s');
ylabel('纬度估计值/°');
subplot(1, 2, 2);
plot(refNav.t(2:end, :), (auxOut_ICAS.L-in_ICAS.L)/pi*180,'LineWidth',3);grid on
xlabel('t/s');
ylabel('纬度估计误差/°');

%% 显示或存储
DataFilePath = [in_sim.CWD '\data\' int2str(in_sim.curGrpIdx) '_' int2str(in_sim.curSimIdx)];
if ~isfolder(DataFilePath)
    mkdir(DataFilePath);
end
diary([DataFilePath '\计算结果.txt']);
diary on;
disp(['时间：' datestr(now)]);
disp('解析对准Y轴与北向夹角/度');
disp(out_CARV2.azm(end, 2)/pi*180);
disp('惯性系对准Y轴与北向夹角/度');
disp(out_ICAS.azm(end, 2)/pi*180);
if vehicleIsStatic
    disp('粗对准时加表总误差/μg');
    disp((norm(accOutMean/par_sim.samplePeriod) - par_sim.g)/1e-6/par_sim.g);
    disp('粗对准时陀螺总误差/(°/h)');
    disp((norm(gyroOutMean/par_sim.samplePeriod) - 7.292115000000000e-05)/(pi/3600/180));
end
disp('解析对准姿态误差/″');
disp(phiL_CARV2(end, :)/pi*180*3600);
disp('惯性系对准姿态误差/″');
disp(phiL_ICAS(end, :)/pi*180*3600);
diary off;
save([DataFilePath '\对准输出分析结果'], 'out_trajGen', 'phiL_CARV2', 'phiL_ICAS', 'out_CARV2', 'out_ICAS');
saveallfigs([DataFilePath '\'], 'fig', false);
end